/*********************************************************************************
*            (c) Copyright,Shenzhen Smart Drone UAV Co., Ltd.  
*                        All Rights Reserved  
* 
* File 	 : pinglin.c
* By  	 : hirsi
* Date	 :2019-08-02 18:12:55
* Other	 : 
*  
**********************************************************************************/
#include "sys.h"


#if (PINLING_Q30T)
void pinglin_Q30T_rock_handle(void)
{
	INT16S tmp16s;        
	INT8U yawpitchctr[] = {0x55,0x01,0x00,0x00,0x00,0x00,0x02, 0x00};
	INT8U zoom_out[] = {0x81, 0x01, 0x04, 0x07, 0x37, 0xFF};
	INT8U zoom_in[] = {0x81, 0x01, 0x04, 0x07, 0x27, 0xFF};
	INT8U zoom_stop[] = {0x81, 0x01, 0x04, 0x07, 0x00, 0xFF};

	if ((pcur10->yaw != ahead10.yaw) || (pcur10->pitch != ahead10.pitch)){
		tmp16s = (INT16S)ROCK_TRANS(pcur10->yaw) - 1500;
		yawpitchctr[2] = LOWBYTE(tmp16s);
		yawpitchctr[3] = HIGHBYTE(tmp16s);

		tmp16s = (INT16S)ROCK_TRANS(pcur10->pitch) - 1500;
		yawpitchctr[4] = LOWBYTE(tmp16s);
		yawpitchctr[5] = HIGHBYTE(tmp16s);
		    
		yawpitchctr[7] = GetChkSum_8(yawpitchctr, 7);
		USART_SENDPKG(yawpitchctr, sizeof(yawpitchctr));	
	}

	if (pcur10->focus != ahead10.focus){
		tmp16s = ROCK_TRANS(pcur10->focus);
		if (tmp16s > 1750){
		    USART_SENDPKG(zoom_out, sizeof(zoom_out));
		}else if (tmp16s < 1250){
		    USART_SENDPKG(zoom_in, sizeof(zoom_in));
		}else{
		    USART_SENDPKG(zoom_stop, sizeof(zoom_stop));
		}
	}
}
#endif


#if (PINLING_18T_D1)

void pl_18TD1_speed_rock_handle(void)
{
	INT16S tmp16s;        
	INT8U yawpitchctr[] = {0x55,0x01,0x00,0x00,0x00,0x00,0x02, 0x00};
	INT8U zoom_out[] = {0xFF, 0x01, 0x00, 0x40, 0x00, 0x00, 0x41};
	INT8U zoom_in[] = {0xFF, 0x01, 0x00, 0x20, 0x00, 0x00, 0x21};
	INT8U zoom_stop[] = {0x81, 0x01, 0x04, 0x07, 0x00, 0xFF};

	if ((pcur10->yaw != ahead10.yaw) || (pcur10->pitch != ahead10.pitch)){
		tmp16s = (INT16S)ROCK_TRANS(pcur10->yaw) - 1500;
		yawpitchctr[2] = LOWBYTE(tmp16s);
		yawpitchctr[3] = HIGHBYTE(tmp16s);

		tmp16s = (INT16S)ROCK_TRANS(pcur10->pitch) - 1500;
		yawpitchctr[4] = LOWBYTE(tmp16s);
		yawpitchctr[5] = HIGHBYTE(tmp16s);
		    
		yawpitchctr[7] = GetChkSum_8(yawpitchctr, 7);
		USART_SENDPKG(yawpitchctr, sizeof(yawpitchctr));	
	}

	
	//if (pcur10->focus != ahead10.focus){
	if (loop_delay_ms(&loopdelay[LOOP_ZOOM_HANDLE], 200)){
		tmp16s = ROCK_TRANS(pcur10->focus);
		if (tmp16s > 1750){
		    USART_SENDPKG(zoom_out, sizeof(zoom_out));
		}else if (tmp16s < 1250){
		    USART_SENDPKG(zoom_in, sizeof(zoom_in));
		}else{
		    USART_SENDPKG(zoom_stop, sizeof(zoom_stop));
		}
	}
}

void pl_18TD1_angle_rock_handle(void)
{
	INT16S tmp16s;        
	INT8U yawpitchctr[20] = {0xff, 0x01, 0x0f, 0x10, 0, 0x45, 0x45, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	INT8U zoom_out[] = {0xFF, 0x01, 0x00, 0x40, 0x00, 0x00, 0x41};
	INT8U zoom_in[] = {0xFF, 0x01, 0x00, 0x20, 0x00, 0x00, 0x21};
	INT8U zoom_stop[] = {0x81, 0x01, 0x04, 0x07, 0x00, 0xFF};

	if ((pcur1E->yaw != ahead1E.yaw) || (pcur1E->pitch != ahead1E.pitch)){
		tmp16s = (pcur1E->pitch*50)/11;		
		yawpitchctr[13] = LOWBYTE(tmp16s);
		yawpitchctr[14] = HIGHBYTE(tmp16s);

		tmp16s = (pcur1E->yaw*50)/11;
		yawpitchctr[17] = LOWBYTE(tmp16s);
		yawpitchctr[18] = HIGHBYTE(tmp16s);
		    
		yawpitchctr[19] = GetChkSum_8(&yawpitchctr[4], 15);
		USART_SENDPKG(yawpitchctr, sizeof(yawpitchctr));	
	}

	if (loop_delay_ms(&loopdelay[LOOP_ZOOM_HANDLE], 200)){	
		if (pcur1E->zoom < 0){
		    USART_SENDPKG(zoom_out, sizeof(zoom_out));
		}else if (pcur1E->zoom > 0){
		    USART_SENDPKG(zoom_in, sizeof(zoom_in));
		}else{
		    USART_SENDPKG(zoom_stop, sizeof(zoom_stop));
		}
	}
}

#endif


